﻿#include "videocontroler.h"

// VideoControler 构造函数，初始化定时器和 UI 界面
VideoControler::VideoControler(QObject *parent) :
    QObject(parent),
    timer(new QTimer(this))
{
    // 连接定时器的超时信号到捕获帧的槽函数
    connect(timer, &QTimer::timeout, this, &VideoControler::captureFrame_slot);

    // 初始化 UI 界面
    cameraFrameWidget = new QWidget;
    cameraFrameUi = new Ui::CameraFrame_wg;
    cameraFrameUi->setupUi(cameraFrameWidget);
    cameraFrameWidget->show();

    // 初始化障碍物检测信息
    lastObstacle["left"] = 0;
    lastObstacle["front"] = 0;
    lastObstacle["right"] = 0;
    currentObstacle["left"] = 0;
    currentObstacle["front"] = 0;
    currentObstacle["right"] = 0;
}

// 初始化视频控制器
bool VideoControler::initialize()
{
    int flag;

    // 初始化 SGBM 立体视觉
    flag = sgbmInit();
    if (flag == false)
    {
        initialized_state = false;
        emit initialized_sig();  // 发出初始化失败信号
        return false;
    }

    // 打开摄像头
    flag = open_camera();
    if (flag == false)
    {
        initialized_state = false;
        emit initialized_sig();
        return false;
    }

    // 初始化成功
    initialized_state = true;
    emit initialized_sig();  // 发出初始化成功信号
    return true;
}

// 获取视频控制器初始化状态
bool VideoControler::getVideoControlerInit_state()
{
    return initialized_state;
}

// 初始化 SGBM 参数
bool VideoControler::sgbmInit()
{
    // 设置左右摄像头的内参矩阵和畸变系数
    leftCameraMatrix = (cv::Mat_<double>(3,3) << 520.207383774164, 0, 311.139101424842,
                                                  0, 520.777593122922, 259.859359165463,
                                                  0, 0, 1);

    rightCameraMatrix = (cv::Mat_<double>(3,3) << 518.901752305930, 0, 314.754864927547,
                                                   0, 519.246351311765, 258.251651214956,
                                                   0, 0, 1);

    leftDistortion = (cv::Mat_<double>(5,1) << -0.0184055347122180, 0.0760715672374823, -0.00207536760151470, 0.00240580414808122, 0);
    rightDistortion = (cv::Mat_<double>(5,1) << -0.0243269947872341, 0.0870999762180924, -0.000779545950137808, 0.00417775240687532, 0);

    // 设置旋转矩阵 R 和平移向量 T
    R = (cv::Mat_<double>(3,3) << 0.99999474008612, 0.000308896732794029, -0.00322868129419828,
                                  -0.000292750407495152, 0.999987456165639, 0.00500018085414252,
                                  0.00323018533368414, -0.00499920935585730, 0.999982286747384);

    T = (cv::Mat_<double>(3,1) << -60.4708803947974, 0.146757050703816, -3.68658441325609);

    imageSize = cv::Size(640,480);

    // 计算立体矫正参数，生成映射表
    cv::stereoRectify(leftCameraMatrix, leftDistortion, rightCameraMatrix, rightDistortion, imageSize, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 0, imageSize);
    cv::initUndistortRectifyMap(leftCameraMatrix, leftDistortion, R1, P1, imageSize, CV_16SC2, left_map1, left_map2);
    cv::initUndistortRectifyMap(rightCameraMatrix, rightDistortion, R2, P2, imageSize, CV_16SC2, right_map1, right_map2);

    // 设置 SGBM 参数
    stereo->setMinDisparity(0);
    stereo->setNumDisparities(16 * 4);
    stereo->setBlockSize(8);
    stereo->setP1(8 * 3 * 6 * 6);
    stereo->setP2(48 * 3 * 6 * 6);
    stereo->setDisp12MaxDiff(-1);
    stereo->setPreFilterCap(5);
    stereo->setUniquenessRatio(10);
    stereo->setSpeckleWindowSize(100);
    stereo->setSpeckleRange(100);
    stereo->setMode(cv::StereoSGBM::MODE_HH);

    return true;
}


// 打开摄像头
bool VideoControler::open_camera()
{


    // 打开摄像头
    if (!capture.open(0))
    {
        qDebug() << "[2.1]Error: Could not open camera with VIDEO_COMMAND:"<< VIDEO_COMMAND;
        return false;
    }
    // 检查是否成功打开
    if (!capture.isOpened())
    {
        qDebug() << "[2.2]Error: Camera opened but is not functional.";
        return false;
    }

    // 设置摄像头参数
    capture.set(CV_CAP_PROP_FRAME_WIDTH, 1280);
    capture.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
    capture.set(CV_CAP_PROP_FPS, 30);

    // 设置摄像头帧率
    int time = 1000 / FPS;
    timer->start(time);

    return true;

}

void VideoControler::captureFrame_slot()
{
    cv::Mat frame;
    if (capture.isOpened() && capture.read(frame))
    {
        emit obstacleDetection_sig(frame);  // 发射信号传递帧
    }

}

void VideoControler::start_ObstacleDetection()
{
    connect(this,&VideoControler::obstacleDetection_sig,this,&VideoControler::obstacleDetection_slot);
}

void VideoControler::stop_ObstacleDetection()
{
    disconnect(this,&VideoControler::obstacleDetection_sig,this,&VideoControler::obstacleDetection_slot);
}

void VideoControler::obstacleDetection_slot(cv::Mat &frame)
{
    if(frame.empty())
        return;

    cv::Mat frame1,frame2,imgL,imgR;
    cv::Mat img1Rectified, img2Rectified, dis_color;

    // 分割左画面和右画面
    frame1 = frame(cv::Rect(0,0,640,480));
    frame2 = frame(cv::Rect(640,0,640,480));
    currentFrame = frame1;

    cv::cvtColor(frame1,imgL,cv::COLOR_RGB2GRAY);
    cv::cvtColor(frame2,imgR,cv::COLOR_RGB2GRAY);

    cv::remap(imgL,img1Rectified,left_map1,left_map2,cv::INTER_LINEAR);
    cv::remap(imgR,img2Rectified,right_map1,right_map2,cv::INTER_LINEAR);

    stereo->compute(img1Rectified,img2Rectified,disparity);

    processDisparity();

    //showGreyScaleDepthMap(disparity_8u);

    showColorDepthMap(dis_color);

}


void VideoControler::showGreyScaleDepthMap(cv::Mat& disparity_8u)
{
    disparity_8u = cv::Mat(disparity.rows,disparity.cols,CV_8UC1);
    cv::normalize(disparity,disparity_8u,0, 255, cv::NORM_MINMAX, CV_8UC1);
    cv::imshow("Depth Image", disparity_8u);
}

void VideoControler::showColorDepthMap(cv::Mat& dis_color)
{
    dis_color = cv::Mat(disparity.rows,disparity.cols,CV_8UC1);
    cv::normalize(disparity,dis_color,0,255,cv::NORM_MINMAX, CV_8UC1);
    cv::applyColorMap(dis_color, dis_color, cv::COLORMAP_JET); // 应用伪彩色映射

    QImage qimg = QImage(dis_color.data, dis_color.cols, dis_color.rows, dis_color.step[0], QImage::Format_RGB888);
    cameraFrameUi->label_2->setPixmap(QPixmap::fromImage(qimg.rgbSwapped()));

}


// 创建掩码，掩盖图像的上部分和下部分
cv::Mat VideoControler::createMask(const cv::Size& shape, int topHeight, int bottomHeight)
{
    // 创建一个与图像大小相同的全白掩码
    cv::Mat mask(shape, CV_8UC1, cv::Scalar(255));

    // 将上方区域设置为黑色（掩盖）
    mask(cv::Rect(0, 0, shape.width, topHeight)) = 0;

    // 将下方区域设置为黑色（掩盖）
    mask(cv::Rect(0, shape.height - bottomHeight, shape.width, bottomHeight)) = 0;

    return mask;
}
// 将掩码应用于深度图像
cv::Mat VideoControler::applyMask(const cv::Mat &depthImage, const cv::Mat &mask)
{
    cv::Mat result;
    // 使用按位与操作将掩码应用于深度图像
    cv::bitwise_and(depthImage, depthImage, result, mask);
    return result;
}


void VideoControler::processDisparity()
{

    // 过滤掉视差值过小或过大的区域
    cv::Mat filteredDisparity = disparity.clone();
    cv::Mat mask1 = (filteredDisparity > 400) | (filteredDisparity < 380);
    filteredDisparity.setTo(0,mask1);

    // 创建掩码，掩盖上方和下方区域
    cv::Mat mask = createMask(disparity.size(), disparity.rows * 0.2, disparity.rows * 0.2);

    // 将掩码应用于过滤后的视差图像
    cv::Mat disparityFiltered = applyMask(filteredDisparity, mask);

    // 将过滤后的视差图像转换为 8 位无符号整型
    cv::Mat disparity8u;
    disparityFiltered.convertTo(disparity8u, CV_8U, 1.0 / 16);

    // 对 8 位视差图像进行阈值处理，得到二值图像
    cv::Mat thresholdOutput;
    cv::threshold(disparity8u, thresholdOutput, 10, 255, cv::THRESH_BINARY);

    QImage qimg(thresholdOutput.data, thresholdOutput.cols, thresholdOutput.rows, thresholdOutput.step[0], QImage::Format_Grayscale8);
    cameraFrameUi->label_3->setPixmap(QPixmap::fromImage(qimg.rgbSwapped()));
    // 查找二值图像中的轮廓
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(thresholdOutput, contours, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

    // 计算轮廓的凸包
    std::vector<std::vector<cv::Point>> hull(contours.size());
    for (size_t i = 0; i < contours.size(); i++)
        cv::convexHull(contours[i], hull[i], false);

    // 过滤掉面积小于 1500 的凸包
    std::vector<std::vector<cv::Point>> filteredHull;
    for (const auto& h : hull)
        if (cv::contourArea(h) > 5000)
            filteredHull.push_back(h);

    // 标志位，标记当前帧是否检测到障碍物
    std::map<std::string, bool> detected = {{"left", false}, {"front", false}, {"right", false}};
    // 方向更新标志，防止每帧对同一方向的障碍物重复计数
    std::map<std::string, bool> updated = {{"left", false}, {"front", false}, {"right", false}};

    // 遍历过滤后的每个凸包
    for (const auto& h : filteredHull)
    {
        // 计算轮廓的中心坐标
        cv::Moments m = cv::moments(h);
        int cX = m.m00 != 0 ? static_cast<int>(m.m10 / m.m00) : 0;
        int cY = m.m00 != 0 ? static_cast<int>(m.m01 / m.m00) : 0;

        // 如果轮廓的中心点在有效区域内
        if (mask.at<uchar>(cY, cX) == 255)
        {
            // 判断障碍物位置并更新计数器
            if (cX < disparity.cols / 3 && !updated["left"])
            {
                detected["left"] = true;
                counters["left"]++;
                updated["left"] = true;  // 设置为已更新
            } else if (cX > 2 * disparity.cols / 3 && !updated["right"])
            {
                detected["right"] = true;
                counters["right"]++;
                updated["right"] = true;  // 设置为已更新
            } else if (!updated["front"])
            {
                detected["front"] = true;
                counters["front"]++;
                updated["front"] = true;  // 设置为已更新
            }
        }

    }
    showColorMap(filteredHull);

    if(detected["left"] == false)
    {
        counters_withoutObstacle["left"] ++;
        if(counters_withoutObstacle["left"] == 30)
        {
            currentObstacle["left"] = 0;
            counters_withoutObstacle["left"] = 0;
        }
    }
    if(detected["front"] == false)
    {
        counters_withoutObstacle["front"] ++;
        if(counters_withoutObstacle["front"] == 30)
        {
            currentObstacle["front"] = 0;
            counters_withoutObstacle["front"] = 0;
        }
    }
    if(detected["right"] == false)
    {
        counters_withoutObstacle["right"] ++;
        if(counters_withoutObstacle["right"] == 30)
        {
            currentObstacle["right"] = 0;
            counters_withoutObstacle["right"] = 0;
        }
    }

    // 判断计数器是否达到阈值，确认障碍物并重置计数器
    if (counters["left"] >= THRESHOLD)
    {
        counters["left"] = 0;  // 重置计数器
        counters_withoutObstacle["left"] = 0;
        currentObstacle["left"] = 1;
    }
    if (counters["front"] >= THRESHOLD)
    {
        counters["front"] = 0;  // 重置计数器
        counters_withoutObstacle["front"] = 0;
        currentObstacle["front"] = 1;
    }
    if (counters["right"] >= THRESHOLD)
    {
        counters["right"] = 0;  // 重置计数器
        counters_withoutObstacle["right"] = 0;
        currentObstacle["right"] = 1;
    }

    warningObstacle();

    // 对于未检测到障碍物的方向，逐步衰减计数器，防止瞬时丢失
    for (const auto& direction : {"left", "front", "right"})
        if (!detected[direction] && counters[direction] > 0)
            counters[direction] = std::max(0, counters[direction] - DECAY);
}

void VideoControler::showColorMap(std::vector<std::vector<cv::Point>>& hull)
{
    // 随机颜色绘制轮廓
    cv::Scalar color(rand() % 256, rand() % 256, rand() % 256);
    // 在原图上绘制边框
    cv::drawContours(currentFrame, std::vector<std::vector<cv::Point>>{hull}, -1, color, 2);

    QImage qimg = QImage(currentFrame.data, currentFrame.cols, currentFrame.rows, currentFrame.step[0], QImage::Format_RGB888);
    cameraFrameUi->label_1->setPixmap(QPixmap::fromImage(qimg.rgbSwapped()));
}


void VideoControler::warningObstacle()
{
    qDebug() << currentObstacle["left"] << currentObstacle["front"] << currentObstacle["right"];

    std::map<std::string, int> Obstacle;
    Obstacle["left"] = 0;
    Obstacle["front"] = 0;
    Obstacle["right"] = 0;

    if(currentObstacle["left"]==1 && lastObstacle["left"]==0)
        Obstacle["left"] = 1;
    if(currentObstacle["front"]==1 && lastObstacle["front"] == 0)
        Obstacle["front"] = 1;
    if(currentObstacle["right"]== 1 && lastObstacle["right"] == 0)
        Obstacle["right"] = 1;

    int type = Obstacle["left"]*1 + Obstacle["front"]*2 + Obstacle["right"]*4;

    switch(type)
    {
    case 1:     //001
    {
        qDebug() << "注意左前方";
        emit sendSpeechTeskToController_sig(".注意左前方.",1);
        break;
    }
    case 2:     //010
    {
        qDebug() << "注意前方";
        emit sendSpeechTeskToController_sig(".注意前方.",1);
        break;

    }
    case 3:     //011
    {
        break;

    }
    case 4:     //100
    {
        qDebug() << "注意右前方";
        emit sendSpeechTeskToController_sig(".注意右前方.",1);
        break;

    }
    case 5:     //101
    {
        break;

    }
    case 6:     //110
    {
        break;

    }
    case 7:     //111
    {
        break;

    }

    };

    lastObstacle = currentObstacle;
}
